#ifndef TPMANAGER_H_
#define TPMANAGER_H_
#include "tpItf.hh"

// #include "zuc/nml_intf/zucpos.h"
// #include "kinematics.h"
// #include "motion.h"
#include "stdbool.h"
// #include "errcode.h"
// #include "zuc/nml_intf/zucpos2.h"
#include "tp_jkzuc_data_type.h"
#include <memory>
#include "math.h"
// #include "tp_unique_data_type.hh"

// #define EXPORT_API __attribute((visibility("default")))
// #define __attribute((visibility("hidden")))

// struct TP_STRUCT;
// typedef struct TP_STRUCT_ TP_STRUCT;

// namespace kine
// {
//     class KineItf;
// };

// class JAKADynIntf;

namespace TP
{
//实现类
class TP_Manger : public TP::TP_Itf  
{
public:
    // void tpManager_init() override;
    TP_Manger();
    ~TP_Manger();
    int tpManger_GetCurTpId() override;

    int init(TP_Config_Stable tp_config, TP_Status_In status_jkzuc_in, std::shared_ptr<kine::KineItf> kine, std::shared_ptr<::JAKADynIntf> dyna) override;

    int update_tp_status(TP_Status_In status_jkzuc_in) override;
    int tpManager_tppause() override;

    int tpManager_tpresume() override;

    // int tpManager_tpSetCycleTime(double secs) override;

    void tpManager_tpabort() override;

    int tpManager_tpErrorID() override;

    void tpManager_tpclear() override;

    void tpManager_tpDisableTrack() override;

    // void tpManager_EnableMoveJMoveLBlend(int enable) override;
    // void tpManager_enableMovejMovejBlend(int enable) override;
    void tpManager_SetCollisionOption(CollisionOption* param) override;

    // ============= motion status ===

    // uint8_t tpManager_tpGetIsTcpVelLimited() override;
    // double tpManager_tpGetDistance_to_go() override;
    // double tpManager_tpGetCurrentvel() override;
    // unsigned char tpManger_tpGetEnables() override;

    int tpManager_tpCollision_rebound() override;
    int tpManager_get_rebound_pos() override;
    int tpManager_tpIsDone() override;

    int tpManager_tpIsAbort() override;
    int tpManager_tpIsPausing() override;
    int tpManager_tpSych_singular_dist() override;
    void tpManager_tpResetSych_singular_dist() override;
    // double tpManager_GetCurVel() override;
    double tpManager_GetDesiredVel() override;
    void tpManager_GetcurSegTargetPose(ZucPose* const pos) override;

    // ==================== tc status
    int tpManager_GetMotId(int index) override;
    double tpManager_GetMotProgress(int index) override;
    double tpManager_GetMotEStopProgress(int index) override;
    void tpManager_SetMotProgress(int index, double progess) override;
    int tpManager_GetMotEnables(int index, unsigned char *enables) override;

    int tpManager_GetMotType(int index) override;

    // 0: double S/T override;  1: toppra
    // int tpManager_GetCurTrajType() override;
    double tpManager_Getjerk() override;

    int tpManager_Blend_next() override;
    int tpManager_GetSafetyAcc(int index, double *safety_acc) override;
    int tpManager_GetCurPlanVel(int index, double *currentVel) override;//获取规划主轴的current规划速度
    int tpManager_GetStopDist(int index, double *stop_dist) override;

    // *****************************
    void tpManager_tpSetCollision_rebound(int collision_rebound) override;
    int tpManager_tpSetPos_JPos(ZucPose const* const pos, double* joint_pos) override;
    int tpManager_tpRevertInterpolation() override;
    int tpManager_tpTrajSmooth(int trqConActive, int firfltActive) override;
    int tpManager_tpSetClsnOpt(CollisionOption clsnopt) override;
    int tpManager_tpSetClsnStopAcc(ClsnStopAcc clsn_stop_acc) override;

    void tpManager_tpSetPose(ZucPose const pose) override;
    void tpManager_tpSetLastPose(ZucPose const pose) override;
    void tpGetCurJointPos(double* jpos) override;
    void tpManager_tpSetCurJpos(double* joint_pos) override;
    void tpManager_tpSetFilterJpos(double* joint_pos) override;
    void tpManager_tpGetGoalJointPos(double* jpos) override;
    void tpManager_tpSetGoalPose(ZucPose const pos) override;
    void tpManager_tpGetGoalPose(ZucPose* const pos) override;
    void tpManager_tpGetlastPose(ZucPose* const pos) override;
    void tpManager_tpGetlastJointPos(double* jpos) override;
    void tpManager_tpSetLastJpos(double* jpos) override;
    int tpManager_tpQueueDepth() override;
    int tpManager_tpActiveDepth() override;
    int tpManager_tpGetErrAddLine() override;
    int tpManager_tpDepth() override;
    int tpManager_tpGetExecId() override;

    // int tpManager_setjointnum(int joint_num) override;
    int tpManager_tcqFull() override;
    bool tpManager_tcqEmpty() override;

    double tpManager_calTimeTTraj(double posLen, double vmax, double amax) override;

    // void tpManager_MarkforViolateLimit() override;
    int tpManager_VoilateLimit_Edge(int joint_num, int* pre_check_limit) override;

    int tpManager_tpSetId(int id) override;
    int tpManager_tpSetTermCond(int cond, double tolerance) override;
    // int tpManager_tpSetAmax(double aMax) override;

    // int tpManager_tpSetVlimit(double vLimit) override;
    // int tpManager_tpSetVMax(double vMax, double ini_maxvel) override;

    void tpManager_tpSetmovsTimeSacle(double scale) override;
    // void zucmotSet_CtrFFLAGES(KINEMATICS_FORWARD_FLAGS fflags) override;
    // void zucmotSet_CtrIFLAGES(KINEMATICS_INVERSE_FLAGS iflags) override;

    // =================  IO
    // int tpManager_tpSetDout(char ioType, int index, unsigned char start, unsigned char end) override;

    // int tpManager_tpSetAout(char ioType, int index, unsigned char start, unsigned char end) override;

    int tpManager_tpSetEndForceCond(int axis, EndForceCond endForceCond) override;

    // =================  Motion Cmd
    int tpManager_tpAddJointLine(ZucPose end,
                                        int canon_motion_type,
                                        double vel,
                                        double ini_maxvel,
                                        double acc,
                                        double jerk,
                                        unsigned char enables,
                                        char atspeed,
                                        int indexrotary,
                                        EndCond endCond,
                                        int jointNum) override;

    int tpManager_tpAddMovs(const double* a0, const double* a1, const double* a2, const double* a3, double time) override;
    int tpManager_tpAddLine(ZucPose end,
                                   int canon_motion_type,
                                   double vel,
                                   double ini_maxvel,
                                   double acc,
                                   double orivel,
                                   double oriacc,
                                   double jerk,
                                   unsigned char enables,
                                   char atspeed,
                                   int indexrotary,
                                   int using_abc_vel,
                                   EndCond endCond) override;

    int tpManager_tpAddMovc(ZucPose mid,
                                   ZucPose end,
                                   double vel,
                                   double ini_maxvel,
                                   double acc,
                                   double jerk,
                                   unsigned char enables,
                                   int using_abc_vel,
                                   double circle_count,
                                   EndCond endCond,
                                   int circle_mode) override;

    int tpManager_tpAddRigidTap(ZucPose end, double vel, double ini_maxvel, double acc, unsigned char enables) override;

    int tpManager_tpAddCircle(ZucPose end,
                                     PmCartesian center,
                                     PmCartesian normal,
                                     int turn,
                                     int canon_motion_type,
                                     double vel,
                                     double ini_maxvel,
                                     double acc,
                                     double jerk,
                                     unsigned char enables,
                                     char atspeed) override;

    int tpManager_tpAddLine_toppra(ZucPose end,
                                          int canon_motion_type,
                                          double vel,
                                          double ini_maxvel,
                                          double acc,
                                          double jerk,
                                          double ori_vel,
                                          double ori_acc,
                                          unsigned char enables,
                                          char atspeed,
                                          int indexrotary,
                                          int using_abc_vel,
                                          EndCond endCond,
                                          int already_inpos,
                                          int planner_type,
                                          int jpos_len,
                                          TcPathInfo* path_info,
                                          TcPathInfo* blend_path_info,
                                          const int cur_blend_left,
                                          const int cur_last_blend_len,
                                          const int blend_jpos_num,
                                          ConstraintsToppra* cons_toppra,
                                          const int id) override;

    int tpManager_tpAddJointLine_toppra(ZucPose end,
                                               int canon_motion_type,
                                               double vel,
                                               double ini_maxvel,
                                               double acc,
                                               double jerk,
                                               unsigned char enables,
                                               char atspeed,
                                               int indexrotary,
                                               EndCond endCond,
                                               int jointNum,
                                               int already_inpos,
                                               int planner_type,
                                               int jpos_len,
                                               TcPathInfo* path_info,
                                               TcPathInfo* blend_path_info,
                                               const int cur_blend_left,
                                               const int cur_last_blend_len,
                                               const int blend_jpos_num,
                                               ConstraintsToppra* cons_toppra,
                                               const int id) override;

    int tpManager_tpAddMovc_toppra(ZucPose mid,
                                          ZucPose end,
                                          double vel,
                                          double ini_maxvel,
                                          double acc,
                                          double jerk,
                                          unsigned char enables,
                                          int using_abc_vel,
                                          double circle_count,
                                          int circle_mode,
                                          EndCond endCond,
                                          int already_inpos,
                                          int planner_type,
                                          int jpos_len,
                                          TcPathInfo* path_info,
                                          TcPathInfo* blend_path_info,
                                          const int cur_blend_left,
                                          const int cur_last_blend_len,
                                          const int blend_jpos_num,
                                          ConstraintsToppra* cons_toppra,
                                          const int id) override;

    int tpManager_tpSetTrackLinearCfgEna(unsigned char enable, double pulseEquivalent, PmCartesian conveyorDir) override;

    int tpManager_tpSetTrackCircularCfgEna(unsigned char enable, double pulseEquivalent, PmHomogeneous o_to_world, int rotate_tool) override;

    // E-Stop
    int tpManager_AddRecoverTc(const double* joint_fb_pos, ZucPose* cart_fb_pos, ZucPose* estop_cart_pos, const double* estop_joint_pos) override;
    int tpManager_EstopRecoverUpdate() override;
    void tpManager_tpGetEStopTcCurJointPos(double* jpos) override;

    double tpManager_GetCurEStopProgress() override;
    double tpManager_GetCurEStopTarget() override;
    int tpManager_GetEStopPose(ZucPose* const pos) override;

    void tpManager_InitRecoverTc() override;
    void tpManager_updateTcDataForEStop() override;

    // track conveyor
    void tpManager_GetConveyDir(PmCartesian* dir) override;
    void tpManager_GetConveyO2world(PmHomogeneous* o2world) override;
    int tpManager_GetConveyRotTool() override;
    unsigned char tpManager_GetConveyEnabled() override;
    char tpManager_GetConveyAnyChanged() override;
    int tpManager_calculate_max_acc_stop_delta_progress(double current_vel, double max_acc, double* delta_target) override;

    int tpManager_init_copy_kine(std::shared_ptr<kine::KineItf> kine) override;
    int tpManager_init_copy_dyna(std::shared_ptr<::JAKADynIntf> dyna) override;

    int tpManager_tpinit() override;
    int tpManager_QueueDepth() override;
    double tpManager_cycletime() override;
    ZucPose tpManger_tpGetdtg() override; 
    
    int tp_main_update() override;

protected:
    int kinematicsForward(const ZucPose* toolOffset,
                                 const double* joint,
                                 ZucPose* world,
                                 unsigned long* interp_iflags,
                                 const PmRpy* base_offset,
                                 const ZucPose* user_offset);

    int kinematicsInverse(const ZucPose* toolOffset,
                                 const ZucPose* world,
                                 double* joint,
                                 const unsigned long* interp_iflags,
                                 const PmRpy* base_offset,
                                 const ZucPose* user_offset);

    int kinematicsForward_elbow(double* joint, ZucPose* world, const PmRpy* base_offset, const ZucPose* user_offset);

    int getTa(const double* q, const double* qd, const double* qdd, double* torque);
    // int tpManager_checkSysInput(tpInput_vtable_t *vtable);
    void updatePosFb();
    int tpExtUpdateInterpolation();
    int tpGetCurTrajType();
    int tpGetMotionType();
    int tpManager_tpRunCycle();
    int checkTcpVelLimit();
    int tp_status_out_update();
    int checkElbowVelLimit();
    int tpRewindMovs(double portion, int joint_num, double end_pos);
    int checkJointLimit();
    int tpRewind( ZucPose* const pos, double portion);
    int tpGetPos( ZucPose* const pos);
    int tpUpdateProgress();
    int JointVelLimitflag();
    void MarkforViolateLimit();
    int tpFIRFilter();
    int tp_set_param_from_status_in();

    //可以删除
    void tpManager_SetSafe_stoppingDist(double dist);
    void tpManager_SetSafe_stoppingtime(double time);
    void tpManager_SetSafe_tcpVelLimit(double vel_limit);
    void tpManager_SetSafe_stoppingDistReduce(double dist_reduce);
    void tpManager_SetSafe_stoppingtimeReduce(double time_reduce);
    void tpManager_SetSafe_enableLimitJointVel(int enable);
    void tpManager_SetSafe_tcpVelLimitReduce(double tcpvel_reduce);
    void tpManager_SetSafe_elbowVelLimitReduce(double vel_limit);
    void tpManager_SetArcBlendsParam() ;
    int tpSetVlimit();
    int tpGet_velScale();
    int tpGet_JointTrqLimit();
};
}
#endif